function vec = se3_to_vec(se3)
vec = [se3(3, 2); se3(1, 3); se3(2, 1); se3(1: 3, 4)];
end